/*
 * FuzzyPID.c
 *
 *  Created on: 2021年11月18日
 *      Author: CameronLee
 */


#include "FuzzyPID.h"

//模糊PID规则表
float pRule[7][7] = {/*enb*/	{ KPB, KPB, KPM, KPM, KPS, KZO, KNS },
	                /*enm*/	    { KPB, KPB, KPM, KPS, KZO, KNS, KNM },
	                /*ens*/	    { KPM, KPM, KPM, KPS, KZO, KNS, KNB },
	                /*ezo*/ 	{ KPB, KPM, KPS, KZO, KNS, KNM, KNB },
	                /*eps*/ 	{ KPB, KPM, KPM, KPS, KZO, KNS, KNB },
	                /*epm*/ 	{ KPS, KZO, KPS, KPS, KZO, KNS, KNM },
	                /*epb*/ 	{ KZO, KZO, KPS, KPM, KPS, KZO, KNS }
					};
					            /*ecnb ecnm ecns eczo ecps ecpm ecpb*/





float iRule[7][7] = {/*enb*/{ KPS, KZO, KZO, KNM, KNS, KZO, KZO },
	                /*enm*/	  { KPM, KPS, KZO, KNS, KNS, KZO, KZO },
	                /*ens*/	  { KPB, KPM, KZO, KZO, KZO, KNS, KNM },
	                /*ezo*/ 	{ KPM, KPM, KPS, KZO, KNS, KNM, KNB },
	                /*eps*/ 	{ KPB, KPM, KPM, KPS, KZO, KNS, KNM },
	                /*epm*/ 	{ KPM, KPS, KPS, KPS, KPS, KZO, KNS },
	                /*epb*/ 	{ KPS, KPS, KPS, KPM, KPM, KPS, KZO }};
					                  /*ecnb ecnm ecns eczo ecps ecpm ecpb*/

float dRule[7][7] = {/*enb*/{ KPB, KPM, KPM, KPS, KNM, KZO, KZO },
	                /*enm*/	  { KPM, KPM, KPS, KPS, KZO, KPS, KPS },
	                /*ens*/	  { KPM, KPM, KPS, KPS, KZO, KPS, KPM },
	                /*ezo*/ 	{ KPS, KPS, KPS, KZO, KPM, KPM, KPB },
	                /*eps*/ 	{ KPS, KPS, KZO, KNS, KZO, KPS, KPM },
	                /*epm*/ 	{ KZO, KZO, KNS, KNM, KNS, KZO, KPS },
	                /*epb*/ 	{ KZO, KNS, KNM, KNB, KNM, KNS, KZO }};
					                  /*ecnb ecnm ecns eczo ecps ecpm ecpb*/


//e or ec 的标准论域 【-3，3】
float e[7][3]= {
									{-10,-5,-2}, //NB
									{-5,-2,-1}, //NM
									{-2,-1, 0}, //NS
									{-1, 0, 1}, //ZO
									{ 0, 1, 3}, //PS
									{ 1, 3, 3}, //PM
									{ 3, 10, 20}  //PB
								};

float ec[7][3]= {
									{-10,-3,-2}, //NB
									{-1,-2,-1}, //NM
									{-2,-1, 0}, //NS
									{-1, 0, 1}, //ZO
									{ 0, 1, 2}, //PS
									{ 1, 2, 3}, //PM
									{ 2, 3, 10}  //PB
								};



static	float iError,P_Out_Part,I_Out_Part,D_Out_Part;

/******************
	@funcation	static float membershipFunc(float x , uint8_t which , float y)

	@brief 三角隶属函数 模糊化

	@param  left,center,right
			    which ： 哪一个区间
					 y     :  论域系数
						flag :	0 求e  论域
										1 求ec 论域

	@retval  float center的隶属度

	@PS
	**需计算2个计算隶属度 例如在NB 和 NM 之间时需计算 其NB 和NM 的隶属度
	**但事实上只需要计算一个因为另一个和这个 的 和 是 1
 */
static float membershipFunc(float x , uint8_t which , float y ,int8_t flag)
{
	float left,center,right;
	if(flag == 0)
	{
		left   =  y * e[which][0];
		center =  y * e[which][1];
		right  =  y * e[which][2];

	if ( x >= left && x <= center)
		{
		return (x-left)/(center-left);
		}
	else if ( x > center && x <= right)
		{
		return (right-x)/(right-center);
		}
	return 0;
	}

	else
	left   =  y * ec[which][0];
	center =  y * ec[which][1];
	right  =  y * ec[which][2];

	if ( x >= left && x <= center)
		{
		return (x-left)/(center-left);
		}
	else if ( x > center && x <= right)
		{
		return (right-x)/(right-center);
		}
	return 0;
}



/*******************************

	@funcation:  static void
               Get_Fuzzy_E_Flag_Membership(float x,
                                           FuzzyFlag* flag,
																					 float y
																					)

	@brief    E模糊化

	@para  		  x    ： 输入
							flag ： 状态结构体
							y    ： 论域系数

	@retval: none

*/

static void Get_Fuzzy_E_Flag_Membership(float xi,FuzzyFlag* flag, float yi)
{

	if(ENONE(xi,yi) && ENB(xi,yi) )
	{
		flag->flag1=NB;
		flag->flag2=NB;
		flag->coefficient = 1;
	}
	else if(ENB(xi,yi) && ENM(xi,yi))
	{
		flag->flag1=NB;
		flag->flag2=NM;
		flag->coefficient =  membershipFunc(xi,NB,yi,E_Membership);
	}
	else if(ENM(xi,yi) && ENS(xi,yi))
	{
		flag->flag1=NM;
		flag->flag2=NS;
		flag->coefficient =  membershipFunc(xi,NM,yi,E_Membership);
	}
	else if(ENS(xi,yi) && EZO(xi,yi))
	{
		flag->flag1=NS;
		flag->flag2=ZO;
		flag->coefficient =  membershipFunc(xi,NS,yi,E_Membership);
	}
	else if(EZO(xi,yi) && EPS(xi,yi))
	{
		flag->flag1=ZO;
		flag->flag2=PS;
		flag->coefficient =  membershipFunc(xi,ZO,yi,E_Membership);
	}
	else if(EPS(xi,yi) && EPM(xi,yi))
	{
		flag->flag1=PS;
		flag->flag2=PM;
		flag->coefficient =  membershipFunc(xi,PS,yi,E_Membership);
	}
	else if(EPM(xi,yi)&& EPB(xi,yi))
	{
		flag->flag1=PM;
		flag->flag2=PB;
		flag->coefficient =  membershipFunc(xi,PM,yi,E_Membership);
	}
	else if(EPB(xi,yi) && ENONE(xi,yi))
	{
		flag->flag1=PB;
		flag->flag2=PB;
		flag->coefficient =  1;
	}
}

/*******************************

	@funcation:  static void
               Get_Fuzzy_EC_Flag_Membership(float x,
                                           FuzzyFlag* flag,
																					 float y
																					)

	@brief    E模糊化

	@para  		  x    ： 输入
							flag ： 状态结构体
							y    ： 论域系数

	@retval: none

*/

static void Get_Fuzzy_EC_Flag_Membership(float xi,FuzzyFlag* flag, float yi)
{

	if(ECNONE(xi,yi) && ECNB(xi,yi) )
	{
		flag->flag1=NB;
		flag->flag2=NB;
		flag->coefficient = 1;
	}
	else if(ECNB(xi,yi) && ECNM(xi,yi))
	{
		flag->flag1=NB;
		flag->flag2=NM;
		flag->coefficient =  membershipFunc(xi,NB,yi,EC_Membership);
	}
	else if(ECNM(xi,yi) && ECNS(xi,yi))
	{
		flag->flag1=NM;
		flag->flag2=NS;
		flag->coefficient =  membershipFunc(xi,NM,yi,EC_Membership);
	}
	else if(ECNS(xi,yi) && ECZO(xi,yi))
	{
		flag->flag1=NS;
		flag->flag2=ZO;
		flag->coefficient =  membershipFunc(xi,NS,yi,EC_Membership);
	}
	else if(ECZO(xi,yi) && ECPS(xi,yi))
	{
		flag->flag1=ZO;
		flag->flag2=PS;
		flag->coefficient =  membershipFunc(xi,ZO,yi,EC_Membership);
	}
	else if(ECPS(xi,yi) && ECPM(xi,yi))
	{
		flag->flag1=PS;
		flag->flag2=PM;
		flag->coefficient =  membershipFunc(xi,PS,yi,EC_Membership);
	}
	else if(ECPM(xi,yi)&& ECPB(xi,yi))
	{
		flag->flag1=PM;
		flag->flag2=PB;
		flag->coefficient =  membershipFunc(xi,PM,yi,EC_Membership);
	}
	else if(ECPB(xi,yi) && ECNONE(xi,yi))
	{
		flag->flag1=PB;
		flag->flag2=PB;
		flag->coefficient =  1;
	}
}






/*******************************
	@funcation

	@brief   解模糊

	@para     ex    ： 当前误差输入，
					  ecx    : 误差变化量输入
					  flag  ： 状态结构体
						y      : 论域系数

	@retval : none

 */
void Fuzzy_Controller_Modifucation(float ex,float ecx,PID* sptr)
{
	FuzzyFlag E_struct;
	FuzzyFlag EC_struct;
	Get_Fuzzy_E_Flag_Membership(ex,&E_struct,E_c);
	Get_Fuzzy_EC_Flag_Membership(ecx,&EC_struct,EC_c);
	sptr->KP = (float)  (   pRule[E_struct.flag1][EC_struct.flag1]*E_struct.coefficient*EC_struct.coefficient\
				+ pRule[E_struct.flag1][EC_struct.flag2]*E_struct.coefficient*(1-EC_struct.coefficient)\
				+ pRule[E_struct.flag2][EC_struct.flag1]*(1-E_struct.coefficient)*EC_struct.coefficient\
				+ pRule[E_struct.flag2][EC_struct.flag2]*(1-E_struct.coefficient)*(1-EC_struct.coefficient)\
                  );

	sptr->KI = (float)  (   iRule[E_struct.flag1][EC_struct.flag1]*E_struct.coefficient*EC_struct.coefficient\
				+ iRule[E_struct.flag1][EC_struct.flag2]*E_struct.coefficient*(1-EC_struct.coefficient)\
				+ iRule[E_struct.flag2][EC_struct.flag1]*(1-E_struct.coefficient)*EC_struct.coefficient\
				+ iRule[E_struct.flag2][EC_struct.flag2]*(1-E_struct.coefficient)*(1-EC_struct.coefficient)\
                  );

	sptr->KD = (float)  (   dRule[E_struct.flag1][EC_struct.flag1]*E_struct.coefficient*EC_struct.coefficient\
				+ dRule[E_struct.flag1][EC_struct.flag2]*E_struct.coefficient*(1-EC_struct.coefficient)\
				+ dRule[E_struct.flag2][EC_struct.flag1]*(1-E_struct.coefficient)*EC_struct.coefficient\
				+ dRule[E_struct.flag2][EC_struct.flag2]*(1-E_struct.coefficient)*(1-EC_struct.coefficient)\
                  );

}


/*******************************
	@funcation  void PID_Arg_Init(PID* sptr)

	@brief   参数初始化

	@para     PID* sptr

	@retval : none

 */
void Fuzzy_PID_Arg_Init(PID* sptr)
{
	sptr->LastError = 0;        //上次偏差值
	sptr->Errorsum = 0;        //上上次偏差值
	sptr->P = PID_P_Set;                //比例常数
	sptr->I = PID_I_Set;                //积分常数
	sptr->D = PID_D_Set;                //微分常数
	sptr->KP = 0;
	sptr->KI = 0;
	sptr->KD = 0;
}




// 新测试
float Fuzzy_PID_Contrl(PID* sptr,float deta_T)
{
	  float pout=0;
	  iError = deta_T;	//当前误差  设定的目标值和实际值的偏差
	// iError < 0  超调                iError > 0  未达到

		Fuzzy_Controller_Modifucation(iError,(sptr->LastError-iError),sptr);

	  sptr->P+=A*sptr->KP;
		sptr->I+=B*sptr->KI;
		sptr->D+=C*sptr->KD;

	 	P_Out_Part  =  sptr->P * iError;
		I_Out_Part  =  sptr->I * sptr->Errorsum;
		D_Out_Part  =  sptr->D * (iError - sptr->LastError);

		//积分分离、限幅
		if( (iError > 3)   ||  (iError < -3) )
		{
			pout = P_Out_Part+D_Out_Part;
		}
		else
		{
			if(I_Out_Part<I_Out_Part_MIN)
				I_Out_Part = I_Out_Part_MIN;
				else if(I_Out_Part>I_Out_Part_MAX)
				I_Out_Part = I_Out_Part_MAX;

			pout = P_Out_Part+I_Out_Part+D_Out_Part;
		}

		//数字补偿
		if((iError<0.5)&&(iError>-0.2))
		{

			if((sptr->LastError-iError)<=0.063)
			{
				pout+=2;
				if(((sptr->LastError-iError)<0.06))  //增温0℃时 补偿+1
				pout++;
				if(((sptr->LastError-iError)<=-0.06))
				pout+=1.3;
				if(((sptr->LastError-iError)<=-0.1))
				pout++;
			}
			else if((sptr->LastError-iError)<=0.13)
				pout++;

			// 49.9 ~ 50
			if((iError<0.1)&&(iError>0))
				pout+=1.5;
			// 50 ~ 50.1
			if((iError<0)&&(iError>-0.1))
				pout+=1;

			//   < 49.9
			if((iError>0.1))
				pout+=4;

			//  >50.1
			if((iError<-0.1))
				pout-=1.5;

			//  >50.2
			if((iError<-0.2))
				pout-=2.5;

		}

	 //限幅
		if( (pout >= PID_Pout_MAX)  || iError >= 30 )
		{
			LED1_ON();
			LED2_ON();
			sptr->Errorsum += iError;
			sptr->LastError = iError;
			return PID_Pout_MAX;
		}

		else if( (pout < PID_Pout_MIN) || iError < (-1) )   //限幅
					{
						LED1_OFF();
						LED2_OFF();
						pout = PID_Pout_MIN;
						sptr->Errorsum += iError;
						sptr->LastError = iError;
						return PID_Pout_MIN;
					}
					else
					{
						LED1_ON();
						LED2_OFF();
						sptr->Errorsum += iError;
						sptr->LastError = iError;
						return pout;
					}

}
































